/*
 * Copyright The Slam Authors
 */

#include <chrono>
#include <map>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include "absl/memory/memory.h"
#include "rclcpp/rclcpp.hpp"
#include "slam/mapping/map_builder.h"
#include "slam/transform/rigid_transform.h"
#include "slam/transform/transform.h"
#include "slam_ros/map_editor.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/node.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/node_options.h"
#include "slam_ros/ros_map.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/urdf_reader.h"
#include "slam_ros/utilities.h"

// ROS Message and Service Headers
#include "agv_msgs/msg/init_pose.hpp"
#include "agv_srvs/srv/edit_map.hpp"
#include "agv_srvs/srv/get_map_list.hpp"
#include "agv_srvs/srv/relocate.hpp"
#include "agv_srvs/srv/save_map.hpp"
#include "agv_srvs/srv/set_loaction_mode.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "slam_ros_msgs/msg/mapdata_progress.hpp"
#include "slam_ros_msgs/srv/change_map.hpp"
#include "slam_ros_msgs/srv/localization_task.hpp"
#include "slam_ros_msgs/srv/mapping_task.hpp"
#include "slam_ros_msgs/srv/query_slam_status.hpp"
#include "slam_ros_msgs/srv/set_init_localization_pose.hpp"
#include "slam_ros_msgs/srv/syncmap.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "yaml-cpp/yaml.h"
#include <shared_parameters/shared_params_interface.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>

using namespace std::chrono_literals;

// Helper function to convert yaw to a quaternion message
geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) {
	tf2::Quaternion q;
	q.setRPY(0, 0, yaw);
	return tf2::toMsg(q);
}

class SlamRosNode : public rclcpp::Node {
	public:
		SlamRosNode() : rclcpp::Node("slam_node_interface") {
			// todo: use shared mermery parameter 
			init_parameters();

			// Initialize services
			mapping_task_service_ = create_service<slam_ros_msgs::srv::MappingTask>(
					slam_ros::kMappingTask,
					std::bind(&SlamRosNode::HandleMappingTask, this, std::placeholders::_1, std::placeholders::_2));

			localization_task_service_ = create_service<slam_ros_msgs::srv::LocalizationTask>(
					slam_ros::kLocalizationTask,
					std::bind(&SlamRosNode::HandleLocalizationTask, this, std::placeholders::_1, std::placeholders::_2));

			query_slam_status_service_ = create_service<slam_ros_msgs::srv::QuerySlamStatus>(
					slam_ros::kQuerySlamStatus,
					std::bind(&SlamRosNode::HandleQuerySlamStatus, this, std::placeholders::_1, std::placeholders::_2));

			set_init_localization_pose_service_ = create_service<agv_srvs::srv::Relocate>(
					slam_ros::ksetZoneTagInitPoseTask,
					std::bind(&SlamRosNode::HandleSetNewInitLocalizationPose, this, std::placeholders::_1, std::placeholders::_2));

			localization_mode_service_ = create_service<agv_srvs::srv::SetLoactionMode>(
					slam_ros::kMappingLocalizationTask,
					std::bind(&SlamRosNode::HandleLocalizationModeTask, this, std::placeholders::_1, std::placeholders::_2));

			save_mapping_service_ = create_service<agv_srvs::srv::SaveMap>(
					slam_ros::kSaveMappingTask,
					std::bind(&SlamRosNode::HandleSaveMappingTask, this, std::placeholders::_1, std::placeholders::_2));

			get_map_list_service_ = create_service<agv_srvs::srv::GetMapList>(
					slam_ros::kGetMapListTask,
					std::bind(&SlamRosNode::HandleGetMapListTask, this, std::placeholders::_1, std::placeholders::_2));

			sync_map_service_ = create_service<slam_ros_msgs::srv::Syncmap>(
					"sync_slam_map",
					std::bind(&SlamRosNode::HandleSyncMap, this, std::placeholders::_1, std::placeholders::_2));

			change_map_service_ = create_service<slam_ros_msgs::srv::ChangeMap>(
					"change_map",
					std::bind(&SlamRosNode::HandleChangeMapTask, this, std::placeholders::_1, std::placeholders::_2));

			// Initialize publishers
			save_map_progress_pub_ = create_publisher<slam_ros_msgs::msg::MapdataProgress>("save_map_progress", 1);
			reset_init_pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 1);

			// Initialize subscribers
			initial_pose_sub_ = create_subscription<agv_msgs::msg::InitPose>(
					"/InitPose", 1,
					std::bind(&SlamRosNode::HandleInitialPose, this, std::placeholders::_1));

			// Initialize timer
			save_map_progress_timer_ = create_wall_timer(
					10ms, std::bind(&SlamRosNode::PublishSavemapProgress, this));

			// Initialize other members
			map_file_name_ = get_parameter("map_file_directory").as_string();

			if (get_parameter("slam_localization").as_int() != 0) {
				::slam_ros::loadinitMapYAML(::slam_ros::kCurrentInitMapID, agv_map_id_);
			        map_file_name_ = get_parameter("load_state_filename").as_string() + agv_map_id_;
			        RCLCPP_INFO_STREAM(get_logger(), "set init State from " << map_file_name_ <<".pbstream");
			        slam_status_ = slam_ros::SlamType::LOCALIZATION;
			}

			last_localization_init_pose_.x = -999.0;
			last_localization_init_pose_.y = -999.0;
		}

		void Run() {
			if (get_parameter("slam_mapping").as_int() != 0 && 
					get_parameter("slam_localization").as_int() == 0 && 
					get_parameter("slam_updatemapping").as_int() == 0) {
				RCLCPP_WARN(get_logger(), "Slam mapping will start!");
				slam_ros::ScopedRosLogSink ros_log_sink;
				set_parameter(rclcpp::Parameter("localization", 0));
				RunMapping();
			} else if (get_parameter("slam_mapping").as_int() != 0 && 
					get_parameter("slam_localization").as_int() == 0 && 
					get_parameter("slam_updatemapping").as_int() != 0) {
				RCLCPP_WARN(get_logger(), "Slam update mapping will start!");
				slam_ros::ScopedRosLogSink ros_log_sink;
				set_parameter(rclcpp::Parameter("localization", 0));
				RunUpdateMapping();
			} else if (get_parameter("slam_mapping").as_int() == 0 && 
					get_parameter("slam_localization").as_int() != 0) {
				RCLCPP_WARN(get_logger(), "Slam localization will start!");
				set_parameter(rclcpp::Parameter("localization", 1));
				slam_ros::ScopedRosLogSink ros_log_sink;
				RunLocalization();
			} else {
				RCLCPP_INFO(get_logger(), "Waiting for request_mapping or request_localization");
				RCLCPP_WARN_ONCE(get_logger(), 
					"Mapping flag = %ld, localization flag = %ld, waiting for request_mapping or request_localization!",
					get_parameter("slam_mapping").as_int(),
					get_parameter("slam_localization").as_int());
			}
		}

		void Start() {
	        running_ = true;
	        run_thread_ = std::thread(&SlamRosNode::RunThread, this);
	    }

	    void Stop() {
	        running_ = false;
	        if (run_thread_.joinable()) {
	            run_thread_.join();
	        }
	    }

	private:
		void RunThread() {
	        rclcpp::Rate rate(1.0);
	        while (running_ && rclcpp::ok()) {
	            {
	                std::lock_guard<std::mutex> lock(run_mutex_);
	                Run();
	            }
	            rate.sleep();
	        }
	    }

		// Service handlers
		void HandleMappingTask(
				const std::shared_ptr<slam_ros_msgs::srv::MappingTask::Request> request,
				std::shared_ptr<slam_ros_msgs::srv::MappingTask::Response> response) {
			absl::MutexLock lock(&service_mutex_);
			if (request->enable) {
				if (get_parameter("slam_mapping").as_int() != 0) {
					RCLCPP_WARN(get_logger(), "Wrong operation, the slam is mapping, mapping id is %ld!", 
							get_parameter("slam_mapping").as_int());
					response->status = false;
					return;
				}
				map_file_name_ = request->map_path;
				slam_status_ = slam_ros::SlamType::MAPPING;
				set_parameter(rclcpp::Parameter("slam_mapping", 1));
				set_parameter(rclcpp::Parameter("slam_localization", 0));
				RCLCPP_WARN(get_logger(), "Slam will switch to mapping mode!");
			} else {
				map_file_name_ = request->map_path;
				slam_status_ = slam_ros::SlamType::UNKNOWN;
				set_parameter(rclcpp::Parameter("slam_mapping", 0));
				RCLCPP_WARN(get_logger(), "Slam mapping will over!");
			}
			response->status = true;
		}

		void HandleLocalizationTask(
				const std::shared_ptr<slam_ros_msgs::srv::LocalizationTask::Request> request,
				std::shared_ptr<slam_ros_msgs::srv::LocalizationTask::Response> response) {
			absl::MutexLock lock(&service_mutex_);
			if (request->enable) {
				if (get_parameter("slam_localization").as_int() != 0) {
					RCLCPP_WARN(get_logger(), "Wrong operation, the slam is in localization with map %ld!", 
							get_parameter("slam_localization").as_int());
					response->status = false;
					return;
				}
				map_file_name_ = request->map_path;
				slam_status_ = slam_ros::SlamType::LOCALIZATION;
				set_parameter(rclcpp::Parameter("slam_localization", 1));
				set_parameter(rclcpp::Parameter("slam_mapping", 0));
				RCLCPP_WARN(get_logger(), "slam will switch to localization mode!");
			} else {
				map_file_name_ = " ";
				slam_status_ = slam_ros::SlamType::UNKNOWN;
				set_parameter(rclcpp::Parameter("slam_localization", 0));
				RCLCPP_WARN(get_logger(), "Slam localization will over!");
			}
			response->status = true;
		}

		void HandleQuerySlamStatus(
				const std::shared_ptr<slam_ros_msgs::srv::QuerySlamStatus::Request> request,
				std::shared_ptr<slam_ros_msgs::srv::QuerySlamStatus::Response> response) {
			
			absl::MutexLock lock(&service_mutex_);
			response->slamstatus = static_cast<int>(slam_status_);
			response->mapfile = map_file_name_;
		}

		void HandleSetNewInitLocalizationPose(
				const std::shared_ptr<agv_srvs::srv::Relocate::Request> request,
				std::shared_ptr<agv_srvs::srv::Relocate::Response> response) {
			absl::MutexLock lock(&service_mutex_);

			localization_init_pose_.x = request->point_yaw.x;
			localization_init_pose_.y = request->point_yaw.y;
			localization_init_pose_.z = request->point_yaw.theta;

			// todo: use share memery parameters
			this->get_parameter("/AGVParam/current_floor", param_map_id_);

			RCLCPP_INFO(get_logger(), "global param of zoneTag is %s, old zoneTag is %s", 
				param_map_id_.c_str(), agv_map_id_.c_str());

			// 检查并更新楼层标签
			std::string zoneTag = request->zonetag.data;
			if(!zoneTag.empty() && param_map_id_ == zoneTag) {
				agv_map_id_ = zoneTag;
			} else {
				response->error = "不切换楼层,只进行重定位";
			}

			response->success = true;

			// 更新地图文件名
			map_file_name_ = get_parameter("map_file_directory").as_string() + agv_map_id_;
			RCLCPP_WARN(get_logger(), "localization_init_pose = (%.2f, %.2f, %.2f)", 
				localization_init_pose_.x, localization_init_pose_.y, localization_init_pose_.z);

			received_new_init_pose_ = true;
       	}

		void HandleInitialPose(const agv_msgs::msg::InitPose::SharedPtr pose_msg) {
			if (!pose_msg->active) {
				return;
			}

			std::string param_map_id;
			// todo: use share memery parameters 
			get_parameter("/AGVParam/current_floor", param_map_id);
			RCLCPP_INFO(get_logger(), "global param of zoneTag is %s, old zoneTag is %s", 
					param_map_id.c_str(), agv_map_id_.c_str());

			std::string zoneTag = pose_msg->zonetag;
			if (zoneTag != "" && param_map_id == zoneTag) {
				agv_map_id_ = zoneTag;
			}

			localization_init_pose_.x = pose_msg->x;
			localization_init_pose_.y = pose_msg->y;
			localization_init_pose_.z = pose_msg->theta;

			double abx = std::abs(localization_init_pose_.x - last_localization_init_pose_.x);
			double aby = std::abs(localization_init_pose_.y - last_localization_init_pose_.y);
			double abz = std::abs(localization_init_pose_.z - last_localization_init_pose_.z);

			if (pose_msg->zonetag != agv_map_id_) {
				agv_map_id_ = pose_msg->zonetag;
				map_file_name_ = get_parameter("map_file_directory").as_string() + agv_map_id_;
				RCLCPP_WARN(get_logger(), "localization_init_pose = (%.2f, %.2f, %.2f)", 
						localization_init_pose_.x, localization_init_pose_.y, localization_init_pose_.z);
				received_new_init_pose_ = true;
				last_localization_init_pose_ = localization_init_pose_;
				return;
			} else if (abx > 0.1 || aby > 0.1 || abz > 0.03) {
				auto cur_msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
				cur_msg->pose.pose.position.x = localization_init_pose_.x;
				cur_msg->pose.pose.position.y = localization_init_pose_.y;
				cur_msg->pose.pose.orientation = createQuaternionMsgFromYaw(localization_init_pose_.z);
				cur_msg->header.frame_id = "map";
				reset_init_pose_pub_->publish(*cur_msg);
				last_localization_init_pose_ = localization_init_pose_;
			} else {
				RCLCPP_WARN(get_logger(), "位置重复 不进行重定位");
			}
		}

		void HandleLocalizationModeTask(
				const std::shared_ptr<agv_srvs::srv::SetLoactionMode::Request> request,
				std::shared_ptr<agv_srvs::srv::SetLoactionMode::Response> response) {
			absl::MutexLock lock(&service_mutex_);
			if (request->mode == 1) {
				if (get_parameter("slam_localization").as_int() != 0) {
					if (agv_map_id_ == request->mapid) {
						RCLCPP_WARN(get_logger(), "Wrong operation, the slam is in localization with map %ld!", 
								get_parameter("slam_localization").as_int());
						response->status.code = 1;
						response->status.message = "Wrong operation, the slam is in localization with map!";
					} else {
						map_file_name_ = get_parameter("map_file_directory").as_string() + request->mapid;
						agv_map_id_ = request->mapid;
						set_parameter(rclcpp::Parameter("re_slam_localization", 1));
					}
					return;
				} else {
					map_file_name_ = get_parameter("map_file_directory").as_string() + request->mapid;
					agv_map_id_ = request->mapid;
					set_parameter(rclcpp::Parameter("slam_localization", 1));
					set_parameter(rclcpp::Parameter("slam_mapping", 0));
					RCLCPP_WARN(get_logger(), "slam will switch to localization mode!");
                    slam_status_ = slam_ros::SlamType::LOCALIZATION;

					response->status.code = 0;
					response->status.message = "Success operation, slam will switch to localization mode!";
				}
			} else if (request->mode == 0) {
				if (get_parameter("slam_mapping").as_int() != 0) {
					RCLCPP_WARN(get_logger(), "Wrong operation, the slam is mapping, mapping id is %ld!",
							get_parameter("slam_mapping").as_int());
					response->status.code = 1;
					response->status.message = "Wrong operation, the slam is mapping!";
					return;
				} else {
					if (request->mapid.empty()) {
						map_file_name_ = get_parameter("map_file_directory").as_string() + request->mapid;
						set_parameter(rclcpp::Parameter("slam_mapping", 1));
						set_parameter(rclcpp::Parameter("slam_localization", 0));
						set_parameter(rclcpp::Parameter("slam_updatemapping", 0));
						slam_status_ = slam_ros::SlamType::MAPPING;
						RCLCPP_WARN(get_logger(), "Slam will switch to mapping mode!");
						response->status.code = 0;
						response->status.message = "Success operation, slam will switch to mapping mode!";
					} else {
						map_file_name_ = get_parameter("map_file_directory").as_string() + request->mapid;

						const std::string pbstream_file_name_load = map_file_name_ + ".pbstream";
						if (!slam_ros::fileExists(pbstream_file_name_load.c_str())) {
							RCLCPP_ERROR(get_logger(), "The mapID: (%s) does not exit on the AGV!", 
									request->mapid.c_str());
							response->status.code = 1;
							response->status.message = "The mapID does not exit on the AGV!";
							return;
						} else {
							set_parameter(rclcpp::Parameter("slam_mapping", 1));
							set_parameter(rclcpp::Parameter("slam_localization", 0));
							set_parameter(rclcpp::Parameter("slam_updatemapping", 1));
							slam_status_ = slam_ros::SlamType::UPDATEMAPPING;
							RCLCPP_WARN(get_logger(), "Slam will switch to update mapping mode!");
							response->status.code = 0;
							response->status.message = "Success operation, slam will switch to update mapping mode!";
						}
					}
				}
			}
		}

		void HandleSaveMappingTask(
				const std::shared_ptr<agv_srvs::srv::SaveMap::Request> request,
				std::shared_ptr<agv_srvs::srv::SaveMap::Response> response) {

			if (get_parameter("slam_mapping").as_int() != 1) {
				RCLCPP_WARN(get_logger(), "Wrong operation, the slam is not mapping, mapping id is %ld!",
						get_parameter("slam_mapping").as_int());
				response->status.code = 1;
				response->status.message = "Wrong operation, the slam is not mapping!";
				return;
			} else {
				map_file_name_ = get_parameter("map_file_directory").as_string() + request->name;
				RCLCPP_WARN(get_logger(), "save mapping will start!");
				response->status.code = 0;
				response->status.message = "save mapping will start!";
				set_parameter(rclcpp::Parameter("slam_mapping", 0));
				set_parameter(rclcpp::Parameter("slam_save_mapping", 1));
				set_parameter(rclcpp::Parameter("slam_updatemapping", 0));
				slam_status_ = slam_ros::SlamType::UNKNOWN;
			}
		}

		void HandleGetMapListTask(
				const std::shared_ptr<agv_srvs::srv::GetMapList::Request> request,
				std::shared_ptr<agv_srvs::srv::GetMapList::Response> response) {
			const std::string kMapListPath = std::string(getenv("HOME")) + "/map";
			slam_ros::directoryGetAllFileName(kMapListPath, &response->mapids);

			RCLCPP_WARN(get_logger(), "kMapListPath is %s", kMapListPath.c_str());
			if (response->mapids.size() < 1) {
				return;
			}

			for (size_t i = 0; i < response->mapids.size(); ++i) {
				const std::string suffix = ".png";
				const size_t map_name_len = response->mapids[i].size() - suffix.size();
				const std::string id_name = response->mapids[i].substr(0, map_name_len);
				response->mapids[i] = id_name;
			}
		}

		void HandleSyncMap(
				const std::shared_ptr<slam_ros_msgs::srv::Syncmap::Request> request,
				std::shared_ptr<slam_ros_msgs::srv::Syncmap::Response> response) {
			const std::string map_zip_path = std::string(getenv("HOME")) + "/map/" + request->mapid + ".tar.gz";
			if (request->opcode == 0) {
				int id = slam_ros::get_file_md5(map_zip_path, response->md5code);
				if (id == -1) {
					response->error = "file not exist.";
					response->success = false;
					return;
				}

				if (response->md5code != request->md5code) {
					response->error = "MD5 check faild.";
					response->success = false;
				} else {
					response->error = "MD5 check success";
					response->success = true;
				}
			} else if (request->opcode == 1) {
				int id = slam_ros::get_file_md5(map_zip_path, response->md5code);
				if (id == -1) {
					response->error = "file not exist.";
					response->success = false;
					return;
				}

				if (response->md5code != request->md5code) {
					response->error = "MD5 check faild.";
					response->success = false;
					return;
				}

				const std::string cmd = "tar -zxvf " + map_zip_path + " -C /";
				int ret = system(cmd.c_str());

				if (ret == -1) {
					RCLCPP_ERROR(get_logger(), "decompression faild.");
					response->error = "decompression faild.";
					response->success = false;
					return;
				}

				const std::string chmod_cmd = "echo 'vensin'| sudo -S chmod -R 777 " + std::string(getenv("HOME")) + "/map/";
				int chmod_ret = system(chmod_cmd.c_str());

				if (chmod_ret == -1 || chmod_ret == 256) {
					RCLCPP_ERROR(get_logger(), "cannot chmod map.");
					response->error = "permission faild.";
					response->success = false;
					return;
				}

				response->error = "sync map success";
				response->success = true;
				return;
			} else {
				response->error = "opCode not exist.";
				response->success = false;
			}
		}

		void HandleChangeMapTask(
	            const std::shared_ptr<slam_ros_msgs::srv::ChangeMap::Request> request,
	            std::shared_ptr<slam_ros_msgs::srv::ChangeMap::Response> response) {

	            const std::string map_path = std::string(getenv("HOME")) + "/map/" + request->mapid;
	            const std::string map_new_path = std::string(getenv("HOME")) + "/map/" + request->mapnewid;

			if (request->code.data == 0) { // 删除操作
				// 检查所有必需文件是否存在
				const std::vector<std::pair<std::string, std::string>> files_to_check = {
					{map_path + ".pbstream", "pb file not exist"},
					{map_path + ".png", "png file not exist"},
					{map_path + ".yaml", "yaml file not exist"},
					{map_path + "_rt.yaml", "rt yaml file not exist"},
					{map_path + ".tar.gz", "zip file not exist"}
				};

				for (const auto& [file_path, error_msg] : files_to_check) {
					if (!slam_ros::fileExists(file_path.c_str())) {
						RCLCPP_INFO(get_logger(), "%s: %s", error_msg.c_str(), file_path.c_str());
						response->error = error_msg;
						response->success = false;
						return;
					}
				}

				// 构建删除命令
				const std::vector<std::string> commands = {
					"echo " + request->keys + " | sudo -S rm -f " + map_path + ".pbstream",
					"echo " + request->keys + " | sudo -S rm -f " + map_path + ".png",
					"echo " + request->keys + " | sudo -S rm -f " + map_path + ".yaml",
					"echo " + request->keys + " | sudo -S rm -f " + map_path + "_rt.yaml",
					"echo " + request->keys + " | sudo -S rm -f " + map_path + ".tar.gz"
				};

				// 执行删除命令
				for (const auto& cmd : commands) {
					int ret = system(cmd.c_str());
					if (ret == -1 || ret == 256) {
						RCLCPP_ERROR(get_logger(), "Failed to execute command: %s, error code: %d", cmd.c_str(), ret);
						response->error = "密钥错误";
						response->success = false;
						return;
					}
				}

				response->error = "成功删除地图";
				response->success = true;
			} else if (request->code.data == 1) { // 重命名操作
				// 检查所有必需文件是否存在
				const std::vector<std::pair<std::string, std::string>> files_to_check = {
					{map_path + ".pbstream", "pb file not exist"},
					{map_path + ".png", "png file not exist"},
					{map_path + ".yaml", "yaml file not exist"},
					{map_path + "_rt.yaml", "rt yaml file not exist"}
				};

				for (const auto& [file_path, error_msg] : files_to_check) {
					if (!slam_ros::fileExists(file_path.c_str())) {
						RCLCPP_INFO(get_logger(), "%s: %s", error_msg.c_str(), file_path.c_str());
						response->error = error_msg;
						response->success = false;
						return;
					}
				}

				// 加载YAML文件获取原点坐标
				double or_x = 0.0, or_y = 0.0;
				if (!loadYAML(map_path + ".yaml", or_x, or_y)) {
					RCLCPP_ERROR(get_logger(), "Failed to load YAML file: %s", (map_path + ".yaml").c_str());
					response->error = "yaml load failed!";
					response->success = false;
					return;
				}

				// 构建新文件名
				const std::string new_pb_file = map_new_path + ".pbstream";
				const std::string new_png_file = map_new_path + ".png";
				const std::string new_yaml_file = map_new_path + ".yaml";
				const std::string new_rt_file = map_new_path + "_rt.yaml";
				const std::string new_gz_file = map_new_path + ".tar.gz";
				const std::string config_dir = std::string(getenv("HOME")) + "/map/config/";

				// 执行重命名命令
				const std::vector<std::string> rename_commands = {
					"echo " + request->keys + " | sudo -S mv " + map_path + ".pbstream " + new_pb_file,
					"echo " + request->keys + " | sudo -S mv " + map_path + ".png " + new_png_file,
					"echo " + request->keys + " | sudo -S mv " + map_path + ".yaml " + new_yaml_file,
					"echo " + request->keys + " | sudo -S mv " + map_path + "_rt.yaml " + new_rt_file,
					"echo " + request->keys + " | sudo -S rm -f " + map_path + ".tar.gz"
				};

				for (const auto& cmd : rename_commands) {
					int ret = system(cmd.c_str());
					if (ret == -1 || ret == 256) {
						RCLCPP_ERROR(get_logger(), "Failed to execute command: %s, error code: %d", cmd.c_str(), ret);
						response->error = "密钥错误";
						response->success = false;
						return;
					}
				}

				// 写入新的YAML文件
				const Eigen::Vector2d origin(or_x, or_y);
				::slam_ros::WriteYaml(0.05, origin, new_yaml_file, new_png_file);

				// 创建新的压缩包
				const std::string zip_cmd = "tar -zcvf " + new_gz_file + " " +
				   new_pb_file + " " +
				   new_png_file + " " +
				   new_yaml_file + " " +
				   new_rt_file + " " +
				   config_dir;

				int zip_ret = system(zip_cmd.c_str());
				if (zip_ret == -1) {
					RCLCPP_ERROR(get_logger(), "Failed to create zip file: %s", new_gz_file.c_str());
					response->error = "compress map file failed";
					response->success = false;
					return;
				}

				// 设置文件权限
				const std::string chmod_cmd = "echo " + request->keys + " | sudo -S chmod -R 777 " + std::string(getenv("HOME")) + "/map/";
				int chmod_ret = system(chmod_cmd.c_str());
				if (chmod_ret == -1 || chmod_ret == 256) {
					RCLCPP_ERROR(get_logger(), "Failed to change permissions: %d", chmod_ret);
					response->error = "密钥错误";
					response->success = false;
					return;
				}

				response->error = "rename map success";
				response->success = true;
			} else {
				response->error = "opCode not exist";
				response->success = false;
			}
	        }

		void PublishSavemapProgress() {
			absl::MutexLock lock(&publish_mutex_);
			if (save_map_progress_pub_->get_subscription_count() > 0 && 
					get_parameter("slam_localization").as_int() == 0) {
				save_map_progress_pub_->publish(curr_map_progress_);
			}
		}

		// Run functions for different modes
		void RunMapping() {
			set_mapping_param();
			constexpr double kTfBufferCacheTimeInSeconds = 10.;
			auto tf_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
			auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

			slam_ros::NodeOptions node_options;
			slam_ros::TrajectoryOptions trajectory_options;
			std::tie(node_options, trajectory_options) = slam_ros::LoadOptions(
					get_parameter("configuration_directory").as_string(), 
					get_parameter("mapping_configuration_basename").as_string());


			node_options.warning_log_path = get_parameter("log_warning_path").as_string();
			auto map_builder = absl::make_unique<slam::mapping::MapBuilder>(
					node_options.map_builder_options);

			auto node = std::make_shared<slam_ros::Node>(node_options, std::move(map_builder), tf_buffer.get(),
					get_parameter("collect_metrics").as_bool());

			CHECK(slam_status_ == slam_ros::SlamType::MAPPING);
    		node->SetSlamType(slam_status_);

			const slam::transform::Rigid3d t_laser_base =
				slam_ros::GetTransformFromUrdf(get_parameter("urdf_file").as_string(), 
						get_parameter("laser_link_name").as_string());
			node->SetLasetToBaseTransform(t_laser_base);

			if (get_parameter("start_trajectory_with_default_topics").as_bool()) {
				node->StartTrajectoryWithDefaultTopics(trajectory_options);
			}

			rclcpp::executors::MultiThreadedExecutor executor;
        	executor.add_node(node);

			rclcpp::Rate loop_rate(get_parameter("ros_loop_rate").as_int());
			while (rclcpp::ok()) {
				executor.spin_some();
				if (get_parameter("slam_mapping").as_int() == 0) {
					if (get_parameter("slam_save_mapping").as_int() == 1) {
						RCLCPP_WARN(get_logger(), "Slam mapping will stop!");
						node->FinishAllTrajectories();
						RCLCPP_INFO(get_logger(), "All trajectories finished!");
						node->RunFinalOptimization();
						RCLCPP_INFO(get_logger(), "Final Optimization over!");

						const std::string pbstream_file_name_save = map_file_name_ + ".pbstream";
						curr_map_progress_.current_progress = 25;
						curr_map_progress_.current_mapfile_name = agv_map_id_;
						curr_map_progress_.status = true;
						curr_map_progress_.error = " saving " + agv_map_id_ + " map.";
						node->SerializeState(pbstream_file_name_save, true, true);
						curr_map_progress_ = node->getMapProgress();

						RCLCPP_WARN(get_logger(), "All trajectories finished! Final Optimization over! SerializeState to %s",
								pbstream_file_name_save.c_str());

						set_parameter(rclcpp::Parameter("slam_save_mapping", 0));
					}
					break;
				}
				loop_rate.sleep();
			}

			executor.cancel();
		}

		void RunLocalization() {
			set_location_param();
			constexpr double kTfBufferCacheTimeInSeconds = 10.;
			auto tf_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
			auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

			slam_ros::NodeOptions node_options;
			slam_ros::TrajectoryOptions trajectory_options;
			std::tie(node_options, trajectory_options) = slam_ros::LoadOptions(
					get_parameter("configuration_directory").as_string(), 
					get_parameter("location_configuration_basename").as_string());

			node_options.warning_log_path = get_parameter("log_warning_path").as_string();
			auto map_builder = absl::make_unique<slam::mapping::MapBuilder>(
					node_options.map_builder_options);

			auto node = std::make_shared<slam_ros::Node>(node_options, std::move(map_builder), tf_buffer.get(),
					get_parameter("collect_metrics").as_bool());

			const std::string pbstream_file_name_load = map_file_name_ + ".pbstream";
			if (slam_ros::fileExists(pbstream_file_name_load.c_str())) {
				node->LoadState(pbstream_file_name_load, get_parameter("load_frozen_state").as_bool());

				std::string m_mapParamfilename = get_parameter("map_file_directory").as_string() + "config/init_map.yaml";
				std::ofstream mapParamStreamFiler(m_mapParamfilename);
				YAML::Emitter out(mapParamStreamFiler);
				out << YAML::BeginMap;
				out << YAML::Key << "agv_map_name";
				out << YAML::Value << agv_map_id_;
				out << YAML::EndMap;
				mapParamStreamFiler.close();

				RCLCPP_WARN(get_logger(), "Load State from %s", pbstream_file_name_load.c_str());

				slam::transform::Rigid2d initial_estimate_pose = slam::transform::Rigid2d::Identity();
				const std::string rtyaml_path = map_file_name_ + "_rt.yaml";
				node->loadRTYAML(rtyaml_path);
				loadRTYAML(rtyaml_path);

				CHECK(slam_status_ == slam_ros::SlamType::LOCALIZATION);
        		node->SetSlamType(slam_status_);
				GetInitialLocalizationPose(node_options, &initial_estimate_pose);
				{
					::slam::mapping::proto::InitialTrajectoryPose initial_trajectory_pose;
					initial_trajectory_pose.set_to_trajectory_id(0);
					*initial_trajectory_pose.mutable_relative_pose() =
						slam::transform::ToProto(slam::transform::Embed3D(initial_estimate_pose));
					initial_trajectory_pose.set_timestamp(
							slam::common::ToUniversal(::slam_ros::FromRos(rclcpp::Time(0))));
					*trajectory_options.trajectory_builder_options
						.mutable_initial_trajectory_pose() = initial_trajectory_pose;
				}

				const slam::transform::Rigid3d t_laser_base =
					slam_ros::GetTransformFromUrdf(get_parameter("urdf_file").as_string(), 
							get_parameter("laser_link_name").as_string());
				node->SetLasetToBaseTransform(t_laser_base);

				if (get_parameter("start_trajectory_with_default_topics").as_bool()) {
					node->StartTrajectoryWithDefaultTopics(trajectory_options);
				}

				rclcpp::executors::MultiThreadedExecutor executor;
        		executor.add_node(node);
				rclcpp::Rate loop_rate(get_parameter("ros_loop_rate").as_int());
				while (rclcpp::ok()) {
					executor.spin_some();
					if (get_parameter("slam_localization").as_int() == 0) {
						break;
					}
					if (received_new_init_pose_) {
						break;
					}
					if (get_parameter("re_slam_localization").as_int() == 1) {
						break;
					}
					loop_rate.sleep();
				}
				executor.cancel();
				node->FinishAllTrajectories();
				node->RunFinalOptimization();
			} else {
				RCLCPP_ERROR(get_logger(), "The pbstream file: %s does not exit!", 
						pbstream_file_name_load.c_str());
				set_parameter(rclcpp::Parameter("slam_localization", 0));
			}
			set_parameter(rclcpp::Parameter("re_slam_localization", 0));
		}

		void RunUpdateMapping() {
			constexpr double kTfBufferCacheTimeInSeconds = 10.;
			auto tf_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
			auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

			slam_ros::NodeOptions node_options;
			slam_ros::TrajectoryOptions trajectory_options;
			std::tie(node_options, trajectory_options) = slam_ros::LoadOptions(
					get_parameter("configuration_directory").as_string(), 
					get_parameter("update_mapping_configuration_basename").as_string());

			node_options.warning_log_path = get_parameter("log_warning_path").as_string();
			auto map_builder = absl::make_unique<slam::mapping::MapBuilder>(
					node_options.map_builder_options);

			auto node = std::make_shared<slam_ros::Node>(node_options, std::move(map_builder), tf_buffer.get(),
					get_parameter("collect_metrics").as_bool());

			CHECK(slam_status_ == slam_ros::SlamType::UPDATEMAPPING);
    		node->SetSlamType(slam_status_);

			const slam::transform::Rigid3d t_laser_base =
				slam_ros::GetTransformFromUrdf(get_parameter("urdf_file").as_string(), 
						get_parameter("laser_link_name").as_string());
			node->SetLasetToBaseTransform(t_laser_base);

			const std::string pbstream_file_name_load = map_file_name_ + ".pbstream";
			if (slam_ros::fileExists(pbstream_file_name_load.c_str())) {
				node->LoadState(pbstream_file_name_load, get_parameter("load_frozen_state").as_bool());
				RCLCPP_WARN(get_logger(), "Load State from %s", pbstream_file_name_load.c_str());

				slam::transform::Rigid2d initial_estimate_pose = slam::transform::Rigid2d::Identity();
				GetInitialLocalizationPose(node_options, &initial_estimate_pose);
				{
					::slam::mapping::proto::InitialTrajectoryPose initial_trajectory_pose;
					initial_trajectory_pose.set_to_trajectory_id(0);
					*initial_trajectory_pose.mutable_relative_pose() =
						slam::transform::ToProto(slam::transform::Embed3D(initial_estimate_pose));
					initial_trajectory_pose.set_timestamp(
							slam::common::ToUniversal(::slam_ros::FromRos(rclcpp::Time(0))));
					*trajectory_options.trajectory_builder_options
						.mutable_initial_trajectory_pose() = initial_trajectory_pose;
				}

				std::string rt_yaml_file = map_file_name_ + "_rt.yaml";
				last_edit_yaw_ = loadRTYAML(rt_yaml_file);
			} else {
				RCLCPP_ERROR(get_logger(), "The pbstream file: %s does not exit!", 
						pbstream_file_name_load.c_str());
				set_parameter(rclcpp::Parameter("slam_mapping", 0));
				set_parameter(rclcpp::Parameter("slam_updatemapping", 0));
			}

			if (get_parameter("start_trajectory_with_default_topics").as_bool()) {
				node->StartTrajectoryWithDefaultTopics(trajectory_options);
			}

			rclcpp::executors::MultiThreadedExecutor executor;
        	executor.add_node(node);
			rclcpp::Rate loop_rate(get_parameter("ros_loop_rate").as_int());
			while (rclcpp::ok()) {
				executor.spin_some();
				if (get_parameter("slam_mapping").as_int() == 0) {
					if (get_parameter("slam_save_mapping").as_int() == 1) {
						RCLCPP_WARN(get_logger(), "Slam mapping will stop!");
						node->FinishAllTrajectories();
						RCLCPP_INFO(get_logger(), "All trajectories finished!");
						node->RunFinalOptimization();
						RCLCPP_INFO(get_logger(), "Final Optimization over!");

						const std::string pbstream_file_name_save = map_file_name_ + ".pbstream";
						curr_map_progress_.current_progress = 25;
						curr_map_progress_.current_mapfile_name = agv_map_id_;
						curr_map_progress_.status = true;
						curr_map_progress_.error = " saving " + agv_map_id_ + " map.";

						node->SerializeState(pbstream_file_name_save, true, true);
						curr_map_progress_ = node->getMapProgress();

						RCLCPP_WARN(get_logger(), "All trajectories finished! Final Optimization over! SerializeState to %s",
								pbstream_file_name_save.c_str());

						if (last_edit_yaw_ != 0.0 || 
								(map_init_pose_from_yaml_.translation().x() != 0.0) ||
								map_init_pose_from_yaml_.translation().y() != 0.0) {

							auto client = this->create_client<agv_srvs::srv::EditMap>("edit_map");

							if (!client->wait_for_service(5s)) {
								RCLCPP_ERROR(get_logger(), "EditMap service not available");
								return;
							}

							auto request = std::make_shared<agv_srvs::srv::EditMap::Request>();
							request->mapid = agv_map_id_;
							request->transform_action.angle = last_edit_yaw_;
							request->transform_action.x = map_init_pose_from_yaml_.translation().x();
							request->transform_action.y = map_init_pose_from_yaml_.translation().y();
							request->transform_action.operate = true;
							request->erase_action.operate = false;

							auto callback = [this](rclcpp::Client<agv_srvs::srv::EditMap>::SharedFuture future) {
							    if (future.valid()) {
							        auto response = future.get();
							        RCLCPP_INFO(get_logger(), "EditMap service call succeeded");
							    } else {
							        RCLCPP_ERROR(get_logger(), "EditMap service call failed");
							    }
							};

							auto future_result = client->async_send_request(request);
						}
						set_parameter(rclcpp::Parameter("slam_save_mapping", 0));
					}
					break;
				}
				loop_rate.sleep();
			}
			executor.cancel();
		}

		// Helper functions
		void GetInitialLocalizationPose(const slam_ros::NodeOptions& node_options,
				slam::transform::Rigid2d* initial_pose) {
			auto& initial_estimate_pose = *initial_pose;

			if (received_new_init_pose_) {
				absl::MutexLock lock(&service_mutex_);
				initial_estimate_pose = slam::transform::Project2D(map_init_pose_from_yaml_).inverse() *
					slam::transform::Rigid2d(
							Eigen::Vector2d(localization_init_pose_.x, localization_init_pose_.y),
							localization_init_pose_.z);
				received_new_init_pose_ = false;
				RCLCPP_WARN(get_logger(), "Recived Initial pose (%.2f, %.2f, %.2f)", 
						initial_estimate_pose.translation().x(),
						initial_estimate_pose.translation().y(),
						initial_estimate_pose.rotation().angle());
				return;
			}

			if (true || node_options.initial_pose_time_interval_sec > 1) {
				const std::string& file_name = slam_ros::kCurrentPose;
				const std::string delimiter = ";";
				std::vector<std::vector<std::string>> all_str_ptr;
				slam_ros::LoadDataFromTxt(file_name, delimiter, &all_str_ptr);
				if (all_str_ptr.size() != 1) {
					RCLCPP_ERROR(get_logger(), "Read inital pose %s failed!", file_name.c_str());
				} else {
					const auto& pose_str = all_str_ptr.front();
					if (pose_str.size() != 4) {
						RCLCPP_ERROR(get_logger(), "The pose format is wrong!");
					} else {
						double pose_time = atof(pose_str[0].c_str());
						double pose_x = atof(pose_str[1].c_str());
						double pose_y = atof(pose_str[2].c_str());
						double pose_yaw = atof(pose_str[3].c_str());
						const double delta_t = now().seconds() - pose_time;
						if (delta_t < node_options.initial_pose_time_interval_sec ||
								node_options.initial_pose_time_interval_sec == -1) {
							absl::MutexLock lock(&service_mutex_);
							initial_estimate_pose = slam::transform::Rigid2d(
									Eigen::Vector2d(pose_x, pose_y), pose_yaw);
							RCLCPP_WARN(get_logger(), "Get Initial pose from %s: (%.2f, %.2f, %.2f)", 
									file_name.c_str(),
									initial_estimate_pose.translation().x(),
									initial_estimate_pose.translation().y(),
									initial_estimate_pose.rotation().angle());
							return;
						} else {
							RCLCPP_ERROR(get_logger(), "The inital pose in file is too old, delta_t: %.2f", delta_t);
						}
					}
				}
			}

			get_parameter("initial_pose_x", localization_init_pose_.x);
			get_parameter("initial_pose_y", localization_init_pose_.y);
			get_parameter("initial_pose_yaw", localization_init_pose_.z);
			initial_estimate_pose = slam::transform::Rigid2d(
					Eigen::Vector2d(localization_init_pose_.x, localization_init_pose_.y),
					localization_init_pose_.z);
			RCLCPP_WARN(get_logger(), "Default Initial pose (%.2f, %.2f, %.2f)", 
					initial_estimate_pose.translation().x(),
					initial_estimate_pose.translation().y(),
					initial_estimate_pose.rotation().angle());
		}


		bool init_parameters() {
			// get shared memery parameters
			if (!SharedParamsInterface::initialize()) {
				RCLCPP_ERROR(get_logger(), "Failed to initialize shared memory interface");
				return false;
			}

			try { 
				auto params = SharedParamsInterface::getNodeParams("shared_parameters");
				if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
					RCLCPP_INFO(get_logger(), "Error get shared parameters");
					return false;
				} 

				params = SharedParamsInterface::getNodeParams("lidar_guidance");
				for (const auto& [name, value] : params) {
					if (name.find("lidar_info") != std::string::npos) 
                    	continue;

					RCLCPP_INFO_STREAM(get_logger(), "get param:" << name << ", value:" << value.toString());
					switch(value.type) {
					case SharedParamsInterface::ParamValue::Type::INT:
					    this->declare_parameter(name, value.getValue<int>());
					    break;
					case SharedParamsInterface::ParamValue::Type::DOUBLE:
					    this->declare_parameter(name, value.getValue<double>());
					    break;
					case SharedParamsInterface::ParamValue::Type::STRING:
					    this->declare_parameter(name, value.getValue<std::string>());
					    break;
					case SharedParamsInterface::ParamValue::Type::BOOL:
					    this->declare_parameter(name, value.getValue<bool>());
					    break;
					default:
					    RCLCPP_WARN_STREAM(get_logger(), "Unsupported parameter type for " << name);
					    break;
					}
				}
			} catch (const std::exception& e) {
				RCLCPP_ERROR_STREAM(get_logger(), "Error reading parameters: " << e.what());
				return false;
			}

			// define other params
			{
				std::string package_share_dir = ament_index_cpp::get_package_share_directory("slam_ros");
				std::string home_dir = std::getenv("HOME");
	                        
				declare_parameter<bool>("collect_metrics", false);
				declare_parameter<std::string>("configuration_directory", package_share_dir + "/configuration_files");
				declare_parameter<std::string>("mapping_configuration_basename", "mapping.lua");
				declare_parameter<std::string>("update_mapping_configuration_basename", "offline_multi_mapping.lua");
				declare_parameter<std::string>("location_configuration_basename", "localization.lua");
				declare_parameter<std::string>("load_state_filename", home_dir + "/map/");
				declare_parameter<bool>("load_frozen_state", true);
				declare_parameter<bool>("start_trajectory_with_default_topics", true);
				declare_parameter<std::string>("save_state_filename", home_dir + "/map/");
				declare_parameter<double>("resolution", 0.05);
				declare_parameter<int>("slam_mapping", 0);
				declare_parameter<int>("slam_localization", 1);
				declare_parameter<int>("slam_updatemapping", 0);
				declare_parameter<int>("re_slam_localization", 0);
				declare_parameter<int>("ros_loop_rate", 100);
				declare_parameter<std::string>("laser_link_name", "laser_link_1");
				declare_parameter<std::string>("urdf_file", package_share_dir + "/urdf/robot.urdf");
				declare_parameter<std::string>("log_path", home_dir + "/Slam_log/localization/info/");
				declare_parameter<std::string>("log_warning_path", home_dir + "/Slam_log/localization/warning/");
				declare_parameter<std::string>("map_file_directory", home_dir + "/map/");
				declare_parameter<int>("slam_save_mapping", 0);
				declare_parameter<int>("localization", 0);
			}
			return true;
		}

		bool set_mapping_param() {
			// # 获取参数
			get_parameter_or("lidar_driver/lidar_model", lidar_model_, 0);
			get_parameter_or("reflector_info/reflector_type", reflector_type_, 1);
			get_parameter_or("reflector_info/max_reflector_range", max_reflector_range_, 10.0);
			get_parameter_or("reflector_info/min_reflector_counter", min_reflector_counter_, 5);
			get_parameter_or("reflector_info/max_reflector_intensity", max_reflector_intensity_, 400);
			get_parameter_or("reflector_info/min_reflector_intensity", min_reflector_intensity_, 10);
			get_parameter_or("reflector_info/reflector_radius", reflector_radius_, 0.025);
			get_parameter_or("reflector_info/reflector_length", reflector_length_, 0.1);
			get_parameter_or("reflector_info/radius_error", radius_error_, 0.01);

			get_parameter_or("use_odom", use_odometry_, true);
			get_parameter_or("use_nav_sat", use_nav_sat_, false);
			get_parameter_or("use_qr_code", use_qr_code_, false);
			get_parameter_or("use_reflector", use_reflector_, false);
			get_parameter_or("use_imu_data", use_imu_data_, false);

			get_parameter_or("duration_sec", duration_sec_, 600);
			get_parameter_or("max_time_seconds", max_time_seconds_, 1.0);
			get_parameter_or("max_angle_radians", max_angle_radians_, 5.0);
			get_parameter_or("max_distance_meters", max_distance_meters_, 0.05);
			get_parameter_or("min_score", min_score_, 0.55);
			get_parameter_or("distance_traveled", distance_traveled_threshold_, 10.0);
			// 构建Lua配置文件内容
			std::string include_file = "include  \"slam_mapping.lua\" ";
			std::string writeBuffer =
				include_file + "\n"
				" " + "\n"
				"options.use_odometry = " + bool2string(use_odometry_) + "\n"
				"options.use_nav_sat = " + bool2string(use_nav_sat_) + "\n"
				"options.use_landmarks = " + bool2string(use_landmarks_) + "\n"
				"options.use_qr_code = " + bool2string(use_qr_code_) + "\n"
				"options.cache_data_time_duration_sec = " + std::to_string(duration_sec_) + "\n"
				" " + "\n"
				"TRAJECTORY_BUILDER_2D.use_imu_data = " + bool2string(use_imu_data_) + "\n"
				"TRAJECTORY_BUILDER_2D.use_odom_in_front = " + bool2string(use_odometry_)  + "\n"
				"TRAJECTORY_BUILDER_2D.use_reflector = " + bool2string(use_reflector_) + "\n"
				" " + "\n"
				"TRAJECTORY_BUILDER_2D.min_reflector_intensity = " + std::to_string(min_reflector_intensity_) + "\n"
				"TRAJECTORY_BUILDER_2D.max_reflector_intensity = " + std::to_string(max_reflector_intensity_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_length = " + std::to_string(reflector_length_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_type = " + std::to_string(reflector_type_)+  "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_radius = " + std::to_string(reflector_radius_) + "\n"
				"TRAJECTORY_BUILDER_2D.max_reflector_range = " + std::to_string(max_reflector_range_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.min_reflector_counter_per_cluster = " + std::to_string(min_reflector_counter_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.radius_error = " + std::to_string(radius_error_) + "\n"
				" " + "\n"
				"return options";

			std::ofstream ofs;
			std::string file_path = get_parameter("configuration_directory").as_string() + "/" + 
				get_parameter("mapping_configuration_basename").as_string();
			ofs.open(file_path.c_str(), std::ios::out);
			if (!ofs.is_open()) {
				return false;
			}
			ofs << writeBuffer.c_str() << std::endl;
			ofs.close();
			return true;
		}

		bool set_location_param() {
			// 获取参数
			get_parameter_or("lidar_driver/lidar_model", lidar_model_, 0);
			get_parameter_or("reflector_info/reflector_type", reflector_type_, 1);
			get_parameter_or("reflector_info/max_reflector_range", max_reflector_range_, 10.0);
			get_parameter_or("reflector_info/min_reflector_counter", min_reflector_counter_, 5);
			get_parameter_or("reflector_info/max_reflector_intensity", max_reflector_intensity_, 400);
			get_parameter_or("reflector_info/min_reflector_intensity", min_reflector_intensity_, 10);
			get_parameter_or("reflector_info/reflector_radius", reflector_radius_, 0.025);
			get_parameter_or("reflector_info/reflector_length", reflector_length_, 0.1);
			get_parameter_or("reflector_info/radius_error", radius_error_, 0.01);

			get_parameter_or("use_odom", use_odometry_, true);
			get_parameter_or("use_nav_sat", use_nav_sat_, false);
			get_parameter_or("use_qr_code", use_qr_code_, false);
			get_parameter_or("use_reflector", use_reflector_, false);
			get_parameter_or("use_imu_data", use_imu_data_, false);
			get_parameter_or("duration_sec", duration_sec_, 600);
			get_parameter_or("max_time_seconds", max_time_seconds_, 1.0);
			get_parameter_or("max_angle_radians", max_angle_radians_, 3.0);
			get_parameter_or("max_distance_meters", max_distance_meters_, 0.05);
			get_parameter_or("min_score", min_score_, 0.55);
			get_parameter_or("distance_traveled", distance_traveled_threshold_, 10.0);

			// 构建Lua配置文件内容
			std::string include_file = "include  \"slam_localization.lua\" ";
			std::string writeBuffer =
				include_file + "\n"
				" " + "\n"
				"options.use_odometry = " + bool2string(use_odometry_) + "\n"
				"options.use_nav_sat = " + bool2string(use_nav_sat_) + "\n"
				"options.use_landmarks = " + bool2string(use_landmarks_) + "\n"
				"options.use_qr_code = " + bool2string(use_qr_code_) + "\n"
				"options.cache_data_time_duration_sec = " + std::to_string(duration_sec_) + "\n"
				" " + "\n"
				"TRAJECTORY_BUILDER_2D.use_imu_data = " + bool2string(use_imu_data_) + "\n"
				"TRAJECTORY_BUILDER_2D.use_odom_in_front = " + bool2string(use_odometry_) + "\n"
				"TRAJECTORY_BUILDER_2D.use_reflector = " + bool2string(use_reflector_) + "\n"
				" " + "\n"
				"TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = " + std::to_string(max_time_seconds_) + "\n"
				"TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(" + std::to_string(max_angle_radians_) + ")\n"
				"TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = " + std::to_string(max_distance_meters_) + "\n"
				" " + "\n"
				"TRAJECTORY_BUILDER_2D.min_reflector_intensity = " + std::to_string(min_reflector_intensity_) + "\n"
				"TRAJECTORY_BUILDER_2D.max_reflector_intensity = " + std::to_string(max_reflector_intensity_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_length = " + std::to_string(reflector_length_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_type = " + std::to_string(reflector_type_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.reflector_radius = " + std::to_string(reflector_radius_) + "\n"
				"TRAJECTORY_BUILDER_2D.max_reflector_range = " + std::to_string(max_reflector_range_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.min_reflector_counter_per_cluster = " + std::to_string(min_reflector_counter_) + "\n"
				"TRAJECTORY_BUILDER_2D.reflector_dector.radius_error = " + std::to_string(radius_error_) + "\n"
				" " + "\n"
				"POSE_GRAPH.use_reflector = " + bool2string(use_reflector_) + "\n"
				"POSE_GRAPH.constraint_builder.min_score = " + std::to_string(min_score_) + "\n"
				"POSE_GRAPH.slam_evaluator.distance_traveled_threshold = " + std::to_string(distance_traveled_threshold_) + "\n"
				" " + "\n"
				"return options";

			// 写入配置文件
			std::ofstream ofs;
			std::string file_path = get_parameter("configuration_directory").as_string() + "/" + 
				get_parameter("location_configuration_basename").as_string();
			ofs.open(file_path, std::ios::out);
			if (!ofs.is_open()) {
				RCLCPP_ERROR(get_logger(), "Failed to open configuration file: %s", file_path.c_str());
				return false;
			}

			ofs << writeBuffer;
			ofs.close();

			RCLCPP_INFO(get_logger(), "Successfully wrote configuration to: %s", file_path.c_str());

			return true;
		}

        bool loadYAML(const std::string& file_name, double& origin_x, double& origin_y) {
            try {
                YAML::Node loaded = YAML::LoadFile(file_name);
                if (!loaded) {
                    RCLCPP_INFO(get_logger(), "Read YAML Failed. Data is not YAML format...");
                    return false;
                }
                
                origin_x = loaded["origin"].as<std::vector<double>>().at(0);
                origin_y = loaded["origin"].as<std::vector<double>>().at(1);
            }
            catch (const YAML::Exception& e) {
                RCLCPP_ERROR(get_logger(), "Read %s error: %s", file_name.c_str(), e.what());
                return false;
            }
            catch (const std::out_of_range& e) {
                RCLCPP_ERROR(get_logger(), "Invalid origin format in %s: %s", file_name.c_str(), e.what());
                return false;
            }
            return true;
        }

	double loadRTYAML(const std::string& file_name) {
		double yaw = 0.0;
		try {
			YAML::Node loaded = YAML::LoadFile(file_name);
			if (!loaded) {
				RCLCPP_INFO(get_logger(), "Read RTYAML Faild. data is not yaml format...");
				return yaw;
			}
			double origin_tx = loaded["translation"].as<std::vector<double>>().at(0);
			double origin_ty = loaded["translation"].as<std::vector<double>>().at(1);
			double origin_tz = loaded["translation"].as<std::vector<double>>().at(2);

			double origin_rw = loaded["rotation"].as<std::vector<double>>().at(0);
			double origin_rx = loaded["rotation"].as<std::vector<double>>().at(1);
			double origin_ry = loaded["rotation"].as<std::vector<double>>().at(2);
			double origin_rz = loaded["rotation"].as<std::vector<double>>().at(3);

			double siny_cosp = 2 * (origin_rw * origin_rz + origin_rx * origin_ry);
			double cosy_cosp = 1 - 2 * (origin_ry * origin_ry + origin_rz * origin_rz);
			yaw = std::atan2(siny_cosp, cosy_cosp);

			const Eigen::Vector3d transtation(origin_tx, origin_ty, origin_tz);
			const Eigen::Quaterniond rotation(origin_rw, origin_rx, origin_ry, origin_rz);

			map_init_pose_from_yaml_ = ::slam::transform::Rigid3d(transtation, rotation);
		} catch (const YAML::Exception& e) {
			RCLCPP_INFO(get_logger(), "Read %s error is %s", file_name.c_str(), e.what());
			return yaw;
		}

		return yaw;
	}

	std::string bool2string(bool value) {
		return value ? "true" : "false";
	}

	// Member variables
	::slam_ros::SlamType slam_status_ = ::slam_ros::SlamType::UNKNOWN;

	std::thread run_thread_;
    std::atomic<bool> running_{false};
    std::mutex run_mutex_;

	std::string map_file_name_;
	std::string agv_map_id_;
	std::string param_map_id_;

	absl::Mutex service_mutex_;
	absl::Mutex publish_mutex_;

	geometry_msgs::msg::Point localization_init_pose_;
	geometry_msgs::msg::Point last_localization_init_pose_;
	double last_edit_yaw_ = 0.0;
	bool received_new_init_pose_ = false;

	slam::transform::Rigid3d map_init_pose_from_yaml_;
	slam_ros_msgs::msg::MapdataProgress curr_map_progress_;

	// ROS2 interfaces
	rclcpp::Service<slam_ros_msgs::srv::MappingTask>::SharedPtr mapping_task_service_;
	rclcpp::Service<slam_ros_msgs::srv::LocalizationTask>::SharedPtr localization_task_service_;
	rclcpp::Service<slam_ros_msgs::srv::QuerySlamStatus>::SharedPtr query_slam_status_service_;
	rclcpp::Service<agv_srvs::srv::Relocate>::SharedPtr set_init_localization_pose_service_;
	rclcpp::Service<agv_srvs::srv::SetLoactionMode>::SharedPtr localization_mode_service_;
	rclcpp::Service<agv_srvs::srv::SaveMap>::SharedPtr save_mapping_service_;
	rclcpp::Service<agv_srvs::srv::GetMapList>::SharedPtr get_map_list_service_;
	rclcpp::Service<slam_ros_msgs::srv::Syncmap>::SharedPtr sync_map_service_;
	rclcpp::Service<slam_ros_msgs::srv::ChangeMap>::SharedPtr change_map_service_;

	rclcpp::Publisher<slam_ros_msgs::msg::MapdataProgress>::SharedPtr save_map_progress_pub_;
	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr reset_init_pose_pub_;

	rclcpp::Subscription<agv_msgs::msg::InitPose>::SharedPtr initial_pose_sub_;

	rclcpp::TimerBase::SharedPtr save_map_progress_timer_;

	// Parameters for SLAM configuration
	int lidar_model_ = 0;
	bool use_odometry_ = true;
	bool use_nav_sat_ = false;
	bool use_landmarks_ = false;
	bool use_qr_code_ = true;
	int duration_sec_ = 600;
	bool use_imu_data_ = false;
	bool use_reflector_ = false;
	double max_time_seconds_ = 1.0;
	double max_angle_radians_ = 3;
	double max_distance_meters_ = 0.05;
	int min_reflector_intensity_ = 225;
	int max_reflector_intensity_ = 400;
	double reflector_length_ = 0.16;
	int reflector_type_ = 1;
	double reflector_radius_ = 0.0375;
	double max_reflector_range_ = 8;
	int min_reflector_counter_ = 4;
	double radius_error_ = 0.01;
	double min_score_ = 0.55;
	double distance_traveled_threshold_ = 10;
};

int main(int argc, char** argv) {
	rclcpp::init(argc, argv);
	auto slam_node = std::make_shared<SlamRosNode>();

	slam_node->Start();
	rclcpp::executors::SingleThreadedExecutor executor;
	executor.add_node(slam_node);
	executor.spin();

	slam_node->Stop();
	rclcpp::shutdown();

	return 0;
}
